function [p,gp] = gpSARSA3(p,gp,ox,action,reward,x,next_action,n,t)
%
% [p,gp] = gpSARSA2(p,gp,ox,action,reward,x,next_action,n,t)
%
% gpSARSA(model,gp,xtmin1,xt,rtmin1,utmin1,ut,t)
% xtmin1: state at t-1
% xt: state at t
%
% GPDARSA implementation based on Y. Engel's 2005 dissertation (sparse
% recursive nonparameteric Monte-Carlo GPSARSA algo).
%
% Note: all timeseries start with t==0 in the first entry,
% i.e. var(1) is the value of var at time 0.
%
% Tobias Siegfried, 06/10/2008
%% init
gp.okt = gp.kt; 
gp.oa = gp.a; 
gp.oc = gp.c; 
gp.os = gp.s; 
gp.od = gp.d;
coef = p.gamma(n) * gp.sigma2 / gp.os;
%% start
%if ~next_action
%    keyboard
%end
ktt = kernel(p,x,x,next_action,next_action);
gp.kt  = k(p,gp,x,next_action);
gp.a = gp.Kinv * gp.kt;
delta = ktt - gp.kt' * gp.a;
if delta < 0
    disp('DELTA NEG.')
end
dk = gp.okt - p.gamma(n) * gp.kt;
if p.reisingerImp
    gp.d = coef * gp.od + dk' * gp.alpha - reward; % here in engel's thesis
end
    %gp.a, keyboard
if delta - p.sparseP > 0
    disp(['add to dict for agent ' num2str(n) ' at time ' num2str(t) ' with delta=' num2str(delta)]);
    gp.h = [gp.oa; -p.gamma(n)]; % ok
    dktt = gp.oa' * (gp.okt - 2 * p.gamma(n) * gp.kt) + p.gamma(n)^2 * ktt; % ok
    if ~ p.reisingerImp
        gp.c = coef * [gp.oc;0] - [gp.C * dk;0] + gp.h;
        gp.d = coef * gp.od - dk' * gp.alpha + reward; % is here in Engel's published version (outside loop (above) in Reisinger's code).
        gp.s = (1 + p.gamma(n)^2) * gp.sigma2 + dktt - p.gamma(n) * gp.sigma2 * coef - dk' *gp.C * dk + 2 * coef * gp.oc' * dk;
    else
        gp.c = coef * gp.oc + gp.C * dk - gp.oa;
        gp.c = [gp.c;p.gamma(n)];
        %gp.s = (1 + p.gamma(n)^2) * gp.sigma2 + dktt - p.gamma(n) * gp.sigma2 * coef - dk' * (gp.c + coef * gp.oc + gp.oa);
        gp.s = (1 + p.gamma(n)^2) * gp.sigma2 + dktt - p.gamma(n) * gp.sigma2 * coef - dk' *gp.C * dk + 2 * coef * gp.oc' * dk;
    end
    gp.alpha = [gp.alpha; 0];
    first = [gp.C; zeros(1,size(gp.C,2))];
    gp.C = [first zeros(size(first,1),1)];
    gp.dict = updateD(gp,x,next_action);
    gp.Kinv = updateKInv(gp,delta);
    gp.a = [gp.z; 1]; gp.z = [gp.z; 0]; % this turns into gp.oa later!
    gp.kt = [gp.kt; ktt];  
else
    disp(['no dict change for agent ' num2str(n) ' at time ' num2str(t)]);
    gp.h = gp.oa - p.gamma(n) * gp.a; % ok
    if ~ p.reisingerImp
        gp.c = coef * gp.oc - gp.C * dk + gp.h; % changes
        gp.s = (1 + p.gamma(n)^2) * gp.sigma2 - p.gamma(n) * gp.sigma2 * coef + dk' * (gp.c + coef * gp.oc); % changes
    else
        dktt = gp.h' * dk;
        gp.c = coef * gp.oc + gp.C * dk - gp.h;
        gp.s = (1 + p.gamma(n)^2) * gp.sigma2 - p.gamma(n) * gp.sigma2 * coef - dk' * (gp.c + coef * gp.oc);
    end
end

%keyboard

gp.alpha = gp.alpha + gp.c * gp.d / gp.s; % ok
gp.C = gp.C + 1 / gp.s * gp.c * gp.c';

function KInv = updateKInv(gp,delta)
% updates KInv
dkaa = delta * gp.Kinv + gp.a * gp.a';
KInv = 1 / delta * [[dkaa; -gp.a'] [-gp.a; 1]];

function res = k(p,gp,s,a)
% computes the sensation projected onto the kernelized space spanned by ...
% the sensations up until now, \mathbf{k}_t(x) = (k_(x_1,x),...,k(x_t,x))'
ds = gp.dict(1:p.stateVDim,:);
da = gp.dict(p.stateVDim+1:end,:);
res = kernel(p,ds,s,da,a);










